#include "DF.h"

using namespace common::planning;

//用于路径搜索算法-expand
CostGridVec DistanceMap::GetNeighbours(
    int64_t x, 
    int64_t y) const {
  CostGridVec neighbours;
  ++ x;
  if(!Occupied(x, y)) neighbours.push_back(CostGrid(x, y, 1.0 + PassDifficulty(x, y))); //Occupied(int64_t x, int64_t y) Cost(int64_t x, int64_t y)
  ++ y;
  if(!Occupied(x, y)) neighbours.push_back(CostGrid(x, y, M_SQRT2 + PassDifficulty(x, y)));
  -- x;
  if(!Occupied(x, y)) neighbours.push_back(CostGrid(x, y, 1.0 + PassDifficulty(x, y)));
  -- x;
  if(!Occupied(x, y)) neighbours.push_back(CostGrid(x, y, M_SQRT2 + PassDifficulty(x, y)));
  -- y;
  if(!Occupied(x, y)) neighbours.push_back(CostGrid(x, y, 1.0 + PassDifficulty(x, y)));
  -- y;
  if(!Occupied(x, y)) neighbours.push_back(CostGrid(x, y, M_SQRT2 + PassDifficulty(x, y)));
  ++ x;
  if(!Occupied(x, y)) neighbours.push_back(CostGrid(x, y, 1.0 + PassDifficulty(x, y)));
  ++ x;
  if(!Occupied(x, y)) neighbours.push_back(CostGrid(x, y, M_SQRT2 + PassDifficulty(x, y)));
  return neighbours;
}

CostGridVec DistanceMap::EightNeighbours(int64_t x, int64_t y) const {
  CostGridVec neighbours;
  ++ x;
  if(IsValid(x, y)) neighbours.push_back(CostGrid(x, y, 0.0));
  ++ y;
  if(IsValid(x, y)) neighbours.push_back(CostGrid(x, y, 0.0));
  -- x;
  if(IsValid(x, y)) neighbours.push_back(CostGrid(x, y, 0.0));
  -- x;
  if(IsValid(x, y)) neighbours.push_back(CostGrid(x, y, 0.0));
  -- y;
  if(IsValid(x, y)) neighbours.push_back(CostGrid(x, y, 0.0));
  -- y;
  if(IsValid(x, y)) neighbours.push_back(CostGrid(x, y, 0.0));
  ++ x;
  if(IsValid(x, y)) neighbours.push_back(CostGrid(x, y, 0.0));
  ++ x;
  if(IsValid(x, y)) neighbours.push_back(CostGrid(x, y, 0.0));
  return neighbours;
}

//用于路径搜索算法-heuristic
float DistanceMap::Heuristic(
    const int64_t cur_x, 
    const int64_t cur_y, 
    const int64_t goal_x, 
    const int64_t goal_y) const {
  int64_t min = cur_x > goal_x ? cur_x - goal_x : goal_x - cur_x;
  int64_t max = cur_y > goal_y ? cur_y - goal_y : goal_y - cur_y;
  if (min > max) {
    int64_t temp = min;
    min = max;
    max = temp;
  }
  return ((M_SQRT2-1.0)*min + max);
}

//用于路径搜索算法-根据obstacle_distance计算通行难度
float DistanceMap::PassDifficulty(
    const int64_t neighbour_x, 
    const int64_t neighbour_y) const {
  float squared_distance = GetGrid(neighbour_x, neighbour_y);
  if(squared_distance > thresh_free_)
    return 0;
  else if(squared_distance > thresh_obstacle_)
    return factor_difficulty_ * (thresh_free_ - squared_distance);
  else
    return std::numeric_limits<float>::max();
}

//判断是否被占用
bool DistanceMap::Occupied(
    const int64_t x, 
    const int64_t y) const {
  if(!IsValid(x, y)) {
    return true;
  }
  return GetGrid(x, y) <= thresh_obstacle_;
}

void DistanceMap::PrintMap() const {
  printf("-------------- distance map --------------\n");
  for(int64_t y = height_; y > 0; -- y) {
    for(int64_t x = 0; x < width_; ++ x) {
      printf("%3.0f ", GetGrid(x, y-1) > 99 ? 99 : GetGrid(x, y-1));
    }
    printf("\n");
  }
}

void DistanceMap::PrintMap(const float threshold) const {
  printf("-------------- obstacle map --------------\n");
  for(int64_t y = height_; y > 0; -- y) {
    for(int64_t x = 0; x < width_; ++ x) {
      if(GetGrid(x, y-1) <= threshold)
        printf("*");
      else
        printf(" ");
    }
    printf("\n");
  }
}

//查找区域内可行grid个数
int64_t common::planning::GetFreeCount(
    const DistanceMap& map, 
    const int64_t xmin, 
    const int64_t ymin, 
    const int64_t xmax, 
    const int64_t ymax) {
  int64_t count = 0;
  for(auto i = xmin; i <= xmax; ++ i) {
    for(auto j = ymin; j <= ymax; ++ j) {
      if(!map.Occupied(i, j)) {
        ++ count;
      }
    }
  }
  printf("[DF.cpp] free in (%ld, %ld)--(%ld, %ld), size is %ld\n", xmin, ymin, xmax, ymax, count);
  return count;
}

//查找某点起始连通的可行grid个数
CostGridSet common::planning::GetConnected(
    const DistanceMap& map, 
    const int64_t x, 
    const int64_t y, 
    const int64_t count_thresh) {
  CostGridQueue visited_q;
  CostGridSet expanded_s;
  if(map.IsValid(x, y) && !map.Occupied(x, y)) {
    visited_q.push_back(CostGrid(x, y));
    expanded_s.insert(CostGrid(x, y));
    while(!visited_q.empty() && expanded_s.size() < size_t(count_thresh)) {
      auto pop_grid = visited_q.front();
      visited_q.pop_front();
      
      for(int64_t i = -1; i <= 1; ++ i) {
        for(int64_t j = -1; j <= 1; ++ j) {
          if(i == 0 && j == 0) {
            continue;
          }
          CostGrid grid(pop_grid.x + i, pop_grid.y + j);
          if(map.IsValid(grid.x, grid.y) && 
             !map.Occupied(grid.x, grid.y) && 
             expanded_s.find(grid) == expanded_s.end()) {
            visited_q.push_back(grid);
            expanded_s.insert(grid);
          }
        }
      }
    }
  }
  printf("[DF.cpp] connected of (%ld, %ld) in the map, size is %ld, count_thresh is %ld\n", x, y, expanded_s.size(), count_thresh);
  return expanded_s;
}